#include "precomp.h"

#include "Pathfinding.h"
#include "../Entity/Entity.h"
#include "Grid.h"

namespace Tmpl {

#define MAX_THREADS 9
static int gSize = 0;

int CalcIdx(int x, int y)
{
	const int cellX = ClampAround(x, gSize);
	const int cellY = ClampAround(y, gSize);
	return cellY * gSize + cellX;
}

int CalculateHCost(int from, int to)
{
	int fromY = from / gSize;
	int fromX = from - fromY * gSize;

	int toY = to / gSize;
	int toX = to - toY * gSize;

	return (abs(fromX - toX) + abs(fromY - toY)) * 20;
}

class PathfindingData
{
public:
	void Init()
	{
		int navSize = gSize*gSize;
		FCosts = MALLOC64(navSize, int);
		GCosts = MALLOC64(navSize, int);
		OpenList = MALLOC64(navSize, int);
		ClosedList = MALLOC64(navSize, int);
		mHeap = MALLOC64(navSize, int);
		mInvertedHeap = MALLOC64(navSize, int);
	}

	void Exit()
	{
		FREE64(FCosts);
		FREE64(GCosts);
		FREE64(OpenList);
		FREE64(ClosedList);
		FREE64(mHeap);
		FREE64(mInvertedHeap);
	}

	inline void Clear(int targetNode)
	{
		memset(FCosts, 0, gSize*gSize*sizeof(int));
		mHeapSize = 0;
		mTargetNode = targetNode;
	}

	inline void AddToOpenList(int idx, int parentIdx, int distance)
	{
		int collisionId = Grid::GetCollisionIdFromCell(idx);
		if (collisionId == Grid::COLLISION_NONE)
		{
			//Grid::SetCollisionId(idx, Grid::COLLISION_WATER);
			int gCost = GCosts[parentIdx] + distance;
			if (FCosts[idx] == 0)
			{
				// add new node
				GCosts[idx] = gCost;
				FCosts[idx] = gCost + CalculateHCost(idx, mTargetNode);
				OpenList[idx] = parentIdx;

				// insert into open list
				OpenListPush(idx);
			}
			else if (gCost < GCosts[idx])
			{
				// update existing node
				GCosts[idx] = gCost;
				FCosts[idx] = gCost + CalculateHCost(idx, mTargetNode);
				OpenList[idx] = parentIdx;

				// sort it again
				OpenListUpdate(idx);
			}
		}
	}

	inline void AddNeighborsToOpenList(int parentIdx)
	{
		int y = parentIdx / gSize;
		int x = parentIdx - y * gSize;

		//AddToOpenList(CalcIdx(x-1, y-1), parentIdx, 15);
		AddToOpenList(CalcIdx(x  , y-1), parentIdx, 10);
		//AddToOpenList(CalcIdx(x+1, y-1), parentIdx, 15);
		AddToOpenList(CalcIdx(x+1, y  ), parentIdx, 10);
		//AddToOpenList(CalcIdx(x+1, y+1), parentIdx, 15);
		AddToOpenList(CalcIdx(x  , y+1), parentIdx, 10);
		//AddToOpenList(CalcIdx(x-1, y+1), parentIdx, 15);
		AddToOpenList(CalcIdx(x-1, y  ), parentIdx, 10);
	}
	inline void OpenListPush(int idx)
	{
		mHeap[++mHeapSize] = idx;
		mInvertedHeap[idx] = mHeapSize;
		HeapReorderUpwards(mHeapSize);
	}
	inline int OpenListPop()
	{
		HeapSwap(1, mHeapSize);
	    HeapReorderDownwards(1, mHeapSize-1);
		return mHeap[mHeapSize--];
	}
	inline void OpenListUpdate(int idx)
	{
		HeapReorderUpwards(mInvertedHeap[idx]);
	}

	inline bool isOpenListEmpty()const{return (mHeapSize==0);}

	// F = Evaluation value
	// G = Cost to move to this node from the start
	// H = Heuristic value
	int* FCosts; // F = G + H
	int* GCosts;
	int* OpenList;
	int* ClosedList;
private:
	void HeapSwap(int a, int b)
	{
		int temp = mHeap[a]; 
		mHeap[a] = mHeap[b]; 
		mHeap[b] = temp;

		mInvertedHeap[mHeap[a]] = a; 
		mInvertedHeap[mHeap[b]] = b;
	}

	void HeapReorderUpwards(int idx)
	{
		while ( (idx>1) && (FCosts[mHeap[idx >> 1]] > FCosts[mHeap[idx]]) )
		{      
			HeapSwap(idx >> 1, idx);
			idx >>= 1;
		}
	}

	void HeapReorderDownwards(int idx, int heapSize)
	{
		while ((idx << 1) <= heapSize)
		{
			int child = (idx << 1);

			//set child to smaller of idx's two children
			if ((child < heapSize) && (FCosts[mHeap[child]] > FCosts[mHeap[child+1]]))
			{
				++child;
			}

			//if this idx is larger than its child, swap
			if (FCosts[mHeap[idx]] > FCosts[mHeap[child]])
			{
				HeapSwap(child, idx);
				idx = child;
			}
			else
			{
				break;
			}
		}
	}

	int* mHeap;
	int* mInvertedHeap;
	int mHeapSize;
	int mTargetNode;
};
static PathfindingData gNavData[MAX_THREADS];

void Navigator::Init()
{
	gSize = Grid::GetSize();
	for (int i=0; i<MAX_THREADS; ++i) gNavData[i].Init();
}

void Navigator::Clear()
{
	for (int i=0; i<MAX_THREADS; ++i) gNavData[i].Exit();
}

GoalFollowPath* Navigator::NavigateTo(Entity* owner, Vec2f from, Vec2f to, char thread)
{
	// find start and end cell
	int startCell = Grid::GetCellIdFromPos(from);
	int endCell = Grid::GetCellIdFromPos(to);
	if (startCell == endCell) return 0;

	// Validate starting and ending cell
	int collisionId = Grid::GetCollisionIdFromCell(startCell);
	if (collisionId != Grid::COLLISION_NONE) return 0;

	// shoot a ray from start to end to see if we can already move
	Ray2f ray = Ray2f(from, to);
	Vec2f intersectPos;
	collisionId = Grid::RayIntersectCollisionId(ray, intersectPos);
	if (collisionId == Grid::COLLISION_NONE)
	{
		GoalFollowPath* goal = new GoalFollowPath(owner);
		goal->SetPathPoint(to, 0);
		return goal;
	}

	// setup data
	PathfindingData& data = gNavData[thread];
	data.Clear(endCell);

	// Add the starting node
	data.ClosedList[startCell] = startCell;
	data.AddNeighborsToOpenList(startCell);

	// Do the aStar Search
	int currentCell;
	while (!data.isOpenListEmpty())
	{
		// evaluate next cell 
		currentCell = data.OpenListPop();

		// add it to the closed list
		data.ClosedList[currentCell] = data.OpenList[currentCell];
		if (currentCell == endCell) break;

		// Add neighbors
		data.AddNeighborsToOpenList(currentCell);
	} 

	// Create the path
	if (currentCell == endCell)
	{
		// first iteration, count the path length
		int pathLength = 1;
		int cell = endCell;
		while (cell != startCell)
		{
			cell = data.ClosedList[cell];
			++pathLength;
		}

		// Now create the path
		cell = endCell;
		int pLength = pathLength - 1;
		GoalFollowPath* goal = new GoalFollowPath(owner);
		while (cell != startCell)
		{
			goal->SetPathPoint(Grid::GetCellCenter(cell), --pathLength);
			cell = data.ClosedList[cell];
		}
		goal->SetPathPoint(to, pLength);
		goal->SetPathPoint(Grid::GetCellCenter(startCell), 0);
		return goal;
	}

	return 0;
}


}